﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Visualization.DataModel
{
    public class SFPStepOld
    {
        /// <summary>
        /// Step number - Each step is 25 milliseconds
        /// </summary>
        public int Step { get; set; }
        /// <summary>
        /// Reference Time from start of data logging
        /// </summary>
        public double RefTime { get; set; }
        /// <summary>
        /// Published X value
        /// </summary>
        public double X { get; set; }
        /// <summary>
        /// Published Y value
        /// </summary>
        public double Y { get; set; }
        /// <summary>
        /// Published theta value
        /// </summary>
        public double Theta { get; set; }
        /// <summary>
        /// Particles created for each step
        /// </summary>
        public List<Particle> particles { get; set; }
        /// <summary>
        /// Particle with max probability in the SFP Step
        /// </summary>
        public Particle HighestProbabilityParticle { get; set; }
        public double StandardDeviation, average;

        public SFPStepOld() {
            HighestProbabilityParticle = new Particle(){Probability = 0.0};
        }

        public SFPStepOld(int StepNum, double ReferenceTime)
        {
            this.Step = StepNum;
            this.RefTime = ReferenceTime;
            this.HighestProbabilityParticle = new Particle() { Probability = 0.0 };
        }

        public SFPStepOld(int StepNum, double ReferenceTime, double predictedX, double predictedY, double predictedTheta)
        {
            this.Step = StepNum;
            this.RefTime = ReferenceTime;
            this.X = predictedX;
            this.Y = predictedY;
            this.Theta = predictedTheta;
            this.HighestProbabilityParticle = new Particle() { Probability = 0.0 };
        }

        /// <summary>
        /// Calculate the standard deviation of particles using particle weight
        /// </summary>
        public void calcStandardDeviation()
        {
            double sumOfDerivationAverage = 0;
            particles.ForEach(delegate(Particle p) {
                average += p.Weight;
                sumOfDerivationAverage += Math.Pow(p.Weight, 2.0);
            });
            this.average /= particles.Count;
            sumOfDerivationAverage /= particles.Count;
            this.StandardDeviation = Math.Sqrt(sumOfDerivationAverage - Math.Pow(average, 2.0));
        }

        /// <summary>
        /// Generate probabilities for each particle based on standard deviation and stores particle with highest probability
        /// </summary>
        public void generateProbabilities()
        {
            particles.ForEach(delegate(Particle p) {
                double zScore = (p.Weight - this.average) / this.StandardDeviation;
                double z = -Math.Pow(zScore, 2.0) / 2;
                p.Probability = (1 / Math.Sqrt(2 * Math.PI)) * (Math.Exp(zScore));
                if (p.Probability > HighestProbabilityParticle.Probability)
                    HighestProbabilityParticle = p;
            });
        }
    }
}
